/*
The surface-physics project: http://code.google.com/p/surface-physics/
Adds real world physics based on the Bullet physics engine, to bases in the Orbiter space
flight simulation software.

Written by Abhijit Nandy
License : Apache License 2.0

PhysicsEngine.cpp
Physics Engine Implementation

This file implements a simple physics engine for Orbiter as a part of the Bump
plugin for the Orbiter Flight Simulation software. Currently based off the VehicleDemo program
provided with Bullet(http://continuousphysics.com/Bullet/)

It currently updates only a single physics object : a raycast vehicle
TODO: Needs to be expanded to manage multiple objects

*/


/* Orbiter header for writing to the log */

#include "PhysicsEngine.h"

#include "../BulletBase/Bump_BaseObject.h"



/**
 * Debugging stuff
 */
char boutput[1000]; //separate variable for threading reasons

//Raycast vehicle specific
bool debugVehicleDefined = false;
btRigidBody 		*m_carChassis;
btVehicleRaycaster  *m_vehicleRayCaster;
btRaycastVehicle	*m_vehicle;
btRaycastVehicle::btVehicleTuning	m_tuning; //used to tune wheels, do not remove
btCollisionShape	*m_wheelShape;
btCollisionShape	*chassisBoxShape;
btCompoundShape 	*compoundChassisShape;
btScalar 			mass;

float	gEngineForce = 0.f;
float	gBreakingForce = 0.f;

float	maxEngineForce = 1000.f;//this should be engine/velocity dependent
float	maxBreakingForce = 100.f;

float	gVehicleSteering = 0.f;
float	steeringIncrement = 0.04f;
float	steeringClamp = 0.3f;


/**
 * Msh stuff
 * TODO these are global and will point to last msh loaded : May cause problems later
 */
int* mshIndices;
btVector3* mshVertices;
char strTerrainMshPath[100] = "Meshes\\RoughTerrain.msh"; //used by importMsh()


/**
 * Constructor for the engine, sets up physics and camera variables
 * Will be generalized more later with VehicleDemo stuff removed.
 */
PhysicsEngine::PhysicsEngine()
:
mCameraMode(VCM_FREE),
m_cameraHeight(4.f),
m_minCameraDistance(3.f),
m_maxCameraDistance(10.f),
m_indexVertexArrays(0),
m_vertices(0)
{
	m_cameraPosition = btVector3(30,30,30);

	mFocusBody[VCM_FREE] = NULL;
	mFocusBody[VCM_ORBITER] = NULL;
	mFocusBody[VCM_GUI] = NULL;


	oapiWriteLogV("PhysicsEngine::PhysicsEngine : New engine allocated !!");
}


/**
 * Destructor, does nothing currently
 */
PhysicsEngine::~PhysicsEngine()
{
	//cleanup in the reverse order of creation/initialization

	//remove the rigidbodies from the dynamics world and delete them
	int i;
	for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
	{
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
		{
			delete body->getMotionState();
		}
		m_dynamicsWorld->removeCollisionObject( obj );
		delete obj;
	}

	//delete collision shapes
	for (int j=0;j<m_collisionShapes.size();j++)
	{
		btCollisionShape* shape = m_collisionShapes[j];
		delete shape;
	}

	m_collisionShapes.clear();

	delete m_indexVertexArrays;
	delete m_vertices;

	//delete dynamics world
	delete m_dynamicsWorld;

	//delete solver
	delete m_constraintSolver;

	//delete broadphase
	delete m_overlappingPairCache;

	//delete dispatcher
	delete m_dispatcher;

	delete m_collisionConfiguration;

	//Msh stuff
	delete[] mshIndices;
	delete[] mshVertices;

}


//--------------------- Collision specific code ------------------------

/**
 * Broadphase filter callback struct : used to get the AABBs of ONLY
 * overlapping bodies, rest are got in get_transforms()
 *
 */
struct broadphase_callback : public btOverlapFilterCallback
{
    //Return true when pairs need collision
    virtual bool
    needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const
    {
		bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
		collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);

		btVector3 aabbMin, aabbMax;

		//This would prevent collision between proxy0 and proxy1 inspite of
		//AABB overlap being detected
		//collides = false;
		btRigidBody* boxA = (btRigidBody*)proxy0->m_clientObject;
		btRigidBody* boxB = (btRigidBody*)proxy1->m_clientObject;

		if (boxA != NULL && boxB != NULL) {

			struct rigid_body *rbA = (struct rigid_body *)boxA->getUserPointer();
			struct rigid_body *rbB = (struct rigid_body *)boxB->getUserPointer();

		/*	bu_log("broadphase_callback: %s & %s has overlapping AABBs",
				   rbA->rb_namep, rbB->rb_namep);

			//Get the AABB for body A : will happen multiple times if there are multiple overlaps
			boxA->getAabb(aabbMin, aabbMax);
			VMOVE(rbA->btbb_min, aabbMin);
			VMOVE(rbA->btbb_max, aabbMax);

			bu_log("broadphase_callback: %s (%f,%f,%f):(%f,%f,%f)",
							   rbA->rb_namep, V3ARGS(rbA->btbb_min),
							   	   	   	      V3ARGS(rbA->btbb_max));

			//Get BB length, width, height & BB center
			VSUB2(rbA->btbb_dims, rbA->btbb_max, rbA->btbb_min);
			VCOMB2(rbA->btbb_center, 1, rbA->btbb_min, 0.5, rbA->btbb_dims);

			//Get the AABB for body B : will happen multiple times if there are multiple overlaps
			boxB->getAabb(aabbMin, aabbMax);
			VMOVE(rbB->btbb_min, aabbMin);
			VMOVE(rbB->btbb_max, aabbMax);

			bu_log("broadphase_callback: %s (%f,%f,%f):(%f,%f,%f)",
							   rbB->rb_namep, V3ARGS(rbB->btbb_min),
							   	   	   	   	  V3ARGS(rbB->btbb_max));

			//Get BB length, width, height & BB center
			VSUB2(rbB->btbb_dims, rbB->btbb_max, rbB->btbb_min);
			VCOMB2(rbB->btbb_center, 1, rbB->btbb_min, 0.5, rbB->btbb_dims);*/

		}

		//add some additional logic here that modifies 'collides'
		return collides;
    }
};


/**
 * Narrowphase callback
 *
 */
void
nearphaseCallback(btBroadphasePair& collisionPair,
		   btCollisionDispatcher& dispatcher,
		   const btDispatcherInfo& dispatchInfo)
{
	//oapiWriteLogV("nearphaseCallback: CALLED");

	btRigidBody* rbA = (btRigidBody*)(collisionPair.m_pProxy0->m_clientObject);
    btRigidBody* rbB = (btRigidBody*)(collisionPair.m_pProxy1->m_clientObject);

    //oapiWriteLogV("nearphaseCallback: rbA:%p, rbB:%p", rbA, rbB);

    if (rbA != NULL && rbB != NULL) {

    	// Find and pack the collision structure with collision info
    	CollisionInfo *collisionInfo;

    	// Try A
    	collisionInfo = (CollisionInfo *)rbA->getUserPointer();
    	if(collisionInfo){
    		collisionInfo->colliding = true;

    		CollisionInfo::CollisionMap::iterator collMapIter = collisionInfo->collMap.find(rbB);
			if(collMapIter == collisionInfo->collMap.end()){
				/*oapiWriteLogV("nearphaseCallback: Filling : %s(%s - rbA), rbB is %s",
				    				collisionInfo->strObjectName,
				    				rbA->getCollisionShape()->getName(),
				    				rbB->getCollisionShape()->getName());*/
				// rbA is the one we got this collisionInfo from
				// rbB is the other body in this case which should be recorded
				collisionInfo->collMap[rbB] = 1;

			}
    	}

    	// Try B
    	collisionInfo = (CollisionInfo *)rbB->getUserPointer();
    	if(collisionInfo){
				collisionInfo->colliding = true;

				CollisionInfo::CollisionMap::iterator collMapIter = collisionInfo->collMap.find(rbA);
				if(collMapIter == collisionInfo->collMap.end()){
					/*oapiWriteLogV("nearphaseCallback: Filling : %s(%s - rbB), rbA is %s",
    				collisionInfo->strObjectName,
    				rbB->getCollisionShape()->getName(),
    				rbA->getCollisionShape()->getName());*/
					collisionInfo->collMap[rbA] = 1;
				}

				for(collMapIter =  collisionInfo->collMap.begin();
					collMapIter != collisionInfo->collMap.end(); collMapIter++){
					/*oapiWriteLogV("nearphaseCallback : %s Colliding with %p",
							collisionInfo->strObjectName, collMapIter->first);*/
				}
    	}
    }

    // Only dispatch the Bullet collision information if physics should continue
    dispatcher.defaultNearCallback(collisionPair, dispatcher, dispatchInfo);
}


/**
 * Initialize the physics world, should be empty before sim starts with commands
 * from various BulletBases to insert objects when sim starts
 * and clearing+resetting after sim ends.
 */
void	PhysicsEngine::initPhysics()
{
	//Move this outta here ! gfx stuff
	if(mode == PM_VISUALIZE){
		setTexturing(true);
		setShadows(true);
		setCameraDistance(10.f);
	}


	//General Physics initialization
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
	btVector3 worldMin(-6000,-6000,-6000);
	btVector3 worldMax(6000,6000,6000);
	m_overlappingPairCache = new btAxisSweep3(worldMin,worldMax);
	m_constraintSolver = new btSequentialImpulseConstraintSolver();
	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration);

	// 0 out the default Earth gravity
	m_dynamicsWorld->setGravity(btVector3(0, 0, 0));


	groundShape = NULL;

	//Add a broadphase callback to hook to the AABB detection algos
	/*btOverlapFilterCallback * filterCallback = new broadphase_callback();
	m_dynamicsWorld->getPairCache()->setOverlapFilterCallback(filterCallback);*/

	//Add a nearphase callback to hook to the contact points generation algos
	m_dispatcher->setNearCallback(nearphaseCallback);

	// ------------------- Debug code -------------------------
	// No point add rigid bodies here, they will fall through unless a ground has been defined
}


/**
 *	Called to define the type of ground by Bump_BulletBase::poststep()
 */
int
PhysicsEngine::defineGround(TerrainType terrainType)
{
	//-----Define ground : Use plane, triangle mesh, box, or .msh file for defining the ground --------
	switch(terrainType){
		case TT_PLANE:	  groundShape = new btStaticPlaneShape(btVector3(0,1,0),0); break;
		case TT_TRIMESH:  groundShape = defineTriMeshShape(); break;
		case TT_BOX:	  groundShape = new btBoxShape(btVector3(200, 3.0, 200)); break;
		case TT_MSH:	  groundShape = importMsh(strTerrainMshPath); addSupportPlane(); break;
		default: ;
	}

	if(groundShape){
		m_collisionShapes.push_back(groundShape);

		btTransform tr;
		tr.setIdentity();

		if(terrainType == TT_BOX)
			tr.setOrigin(btVector3(0.f,-3.f,0.f));


		//create ground object
		localCreateRigidBody(0,tr,groundShape);
	}

	//------------------------------ground defined, now other physics objects-------------------------

	//mOrbiterFocusBody = groundRigidBody;


	return 0;
}


/**
 * Adds a mesh shape based on triangles
 * TODO: Extend to full class, generalize for different shapes
 */
btCollisionShape*
PhysicsEngine::defineTriMeshShape(void)
{
	int i;

	const float TRIANGLE_SIZE=20.f;

	//create a triangle-mesh ground
	int vertStride = sizeof(btVector3);
	int indexStride = 3*sizeof(int);

	const int NUM_VERTS_X = 10;
	const int NUM_VERTS_Y = 10;
	const int totalVerts = NUM_VERTS_X*NUM_VERTS_Y;

	const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1);

	m_vertices = new btVector3[totalVerts];
	int*	gIndices = new int[totalTriangles*3];



	for ( i=0;i<NUM_VERTS_X;i++)
	{
		for (int j=0;j<NUM_VERTS_Y;j++)
		{
			float wl = .2f;
			//height set to zero, but can also use curved landscape just uncomment out the code
			float height = 0.f; //fabs(20.f*sinf(float(i)));
			m_vertices[i+j*NUM_VERTS_X].setValue(
				(i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE,
				height,
				(j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE);

		/*	printf("%d, %f, %f, %f\n", i+j*NUM_VERTS_X, m_vertices[i+j*NUM_VERTS_X].getX(),
				m_vertices[i+j*NUM_VERTS_X].getY(),
				m_vertices[i+j*NUM_VERTS_X].getZ());*/
		}
	}

	int index=0;
	for ( i=0;i<NUM_VERTS_X-1;i++)
	{
		for (int j=0;j<NUM_VERTS_Y-1;j++)
		{
			gIndices[index++] = j*NUM_VERTS_X+i;
			gIndices[index++] = j*NUM_VERTS_X+i+1;
			gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;

			gIndices[index++] = j*NUM_VERTS_X+i;
			gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
			gIndices[index++] = (j+1)*NUM_VERTS_X+i;
		}
	}

	/*for(i=0; i<totalTriangles*3; i++)
		printf(" gIndices[%d] : %d\n", i, gIndices[i]); */

	m_indexVertexArrays = new btTriangleIndexVertexArray(totalTriangles,
		gIndices,
		indexStride,
		totalVerts,(btScalar*) &m_vertices[0].x(),vertStride);

	bool useQuantizedAabbCompression = true;
	return new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression);

}


/**
 *	Called to define a test vehicle
 *	DO NOT CALL this from initPhysics() unless a ground has been defined !
 *	The vehicle will fall through.
 */
int
PhysicsEngine::defineVehicle(void)
{

	/**
	 * Wheel parameters : Wheels continue to penetrate triangle mesh, not because of physics model,
	 * but because wheel mesh and physics wheel not aligned(physics wheel is more inwards,
	 * collision mesh wheel more outwards : there is actually no penetration)
	 */
	float	wheelRadius;
	float	wheelWidth;	 //actual width is 2.364(twice)
	float	wheelFriction;
	float	wheelFwdBackDist;
	float   wheelLeftRightDist;
	float	connectionHeight;
	int 	rightIndex;
	int 	upIndex;
	int 	forwardIndex;
	btVector3 wheelDirectionCS0;
	btVector3 wheelAxleCS;

	//Suspension parameters
	float	suspensionStiffness;
	float	suspensionDamping;
	float	suspensionCompression;
	float	rollInfluence;//1.0f;
	float 	suspensionRestLength;

	//Heights and initial location
	float 	initialVehicleHeight; //Height from which vehicle is dropped on terrain at physics start
	float 	chassisHeight; //Height of chassis above vehicle origin
	float 	chassisHalfLength;
	float 	chassisHalfWidth;
	float 	chassisHalfHeight;

	//Vehicle
	m_carChassis = NULL;
	m_vehicle = 0;
	m_wheelShape = 0;
	mass = 1500.f;





	/**
	 * Wheel parameters : Wheels continue to penetrate triangle mesh, not because of physics model,
	 * but because wheel mesh and physics wheel not aligned(physics wheel is more inwards,
	 * collision mesh wheel more outwards : there is actually no penetration)
	 */
	wheelRadius = 1.5f;
	wheelWidth = 1.182f;	 //actual width is 2.364(twice)
	wheelFriction = 1500;
	wheelFwdBackDist = 2.7f;
	wheelLeftRightDist = 1.3f;
	connectionHeight = 1.2f;
	rightIndex = 0;
	upIndex = 1;
	forwardIndex = 2;
	wheelDirectionCS0.setValue(0,-1,0);
	wheelAxleCS.setValue(-1,0,0);

	//Suspension parameters
	suspensionStiffness = 50.f;
	suspensionDamping = 20.0f;
	suspensionCompression = 2.4f;
	rollInfluence = 0.08f;//1.0f;
	suspensionRestLength = 0.9f;

	//Heights and initial location
	initialVehicleHeight = 2.0f; //Height from which vehicle is dropped on terrain at physics start
	chassisHeight = 1.2f; //Height of chassis above vehicle origin
	chassisHalfLength = 3.0f;
	chassisHalfWidth = 2.0f;
	chassisHalfHeight = 1.0f;


	//--------------------------- define vehicle from here ---------------------
	btTransform chassisTrans, localTrans;
	bool isFrontWheel;


	//The compound shape for the chassis
	compoundChassisShape = new btCompoundShape();
	//bumpBase->m_collisionShapes->push_back(compoundChassisShape);

	//The chassis shape to be added to the compound shape
	chassisBoxShape = new btBoxShape(btVector3(chassisHalfWidth,
											chassisHalfHeight,
											chassisHalfLength));
	//bumpBase->m_collisionShapes->push_back(chassisBoxShape);

	//localTrans for the chassis shape
	//It effectively shifts the center of mass with respect to the chassis
	localTrans.setIdentity();
	localTrans.setOrigin(btVector3(0, chassisHeight,0));

	//Add chassis shape to compound shape
	compoundChassisShape->addChildShape(localTrans, chassisBoxShape);

	//Position the chassis inside the vehicle
	chassisTrans.setIdentity();

	//Pads are dug into the ground, will make vessel jump out if not 0ed
	chassisTrans.setOrigin(btVector3(30., 10., 0.));
	m_carChassis = localCreateRigidBody(mass, chassisTrans, compoundChassisShape);

	//Now the wheels, first the cylinder shape for the wheels
	m_wheelShape = new btCylinderShapeX(btVector3(wheelWidth, wheelRadius, wheelRadius));

	//Create vehicle
	m_vehicleRayCaster = new btDefaultVehicleRaycaster(m_dynamicsWorld);
	m_vehicle = new btRaycastVehicle(m_tuning,m_carChassis,m_vehicleRayCaster);

	//Never deactivate the vehicle
	m_carChassis->setActivationState(DISABLE_DEACTIVATION);

	m_dynamicsWorld->addVehicle(m_vehicle);

	//choose coordinate system
	m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex);

	//forward left wheel
	isFrontWheel=true;
	btVector3 connectionPointCS0(wheelLeftRightDist, connectionHeight, wheelFwdBackDist);
	m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);

	//forward right wheel
	connectionPointCS0 = btVector3(-wheelLeftRightDist, connectionHeight, wheelFwdBackDist);
	m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);

	isFrontWheel = false;
	//backward left wheel
	connectionPointCS0 = btVector3(wheelLeftRightDist, connectionHeight, -wheelFwdBackDist);
	m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);

	//backward right wheel
	connectionPointCS0 = btVector3(-wheelLeftRightDist, connectionHeight, -wheelFwdBackDist);
	m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);

	//Set wheel properties for all 4 wheels
	for (int i=0;i<m_vehicle->getNumWheels();i++){
		btWheelInfo& wheel = m_vehicle->getWheelInfo(i);
		wheel.m_suspensionStiffness = suspensionStiffness;
		wheel.m_wheelsDampingRelaxation = suspensionDamping;
		wheel.m_wheelsDampingCompression = suspensionCompression;
		wheel.m_frictionSlip = wheelFriction;
		wheel.m_rollInfluence = rollInfluence;
	}

	//mOrbiterFocusBody = m_carChassis;
	debugVehicleDefined = true;


	return 0;
}

void
PhysicsEngine::updateVehicle(void)
{
	if (debugVehicleDefined) {
		int wheelIndex = 2;
		m_vehicle->applyEngineForce(gEngineForce,wheelIndex);
		m_vehicle->setBrake(gBreakingForce,wheelIndex);
		wheelIndex = 3;
		m_vehicle->applyEngineForce(gEngineForce,wheelIndex);
		m_vehicle->setBrake(gBreakingForce,wheelIndex);


		wheelIndex = 0;
		m_vehicle->setSteeringValue(gVehicleSteering,wheelIndex);
		wheelIndex = 1;
		m_vehicle->setSteeringValue(gVehicleSteering,wheelIndex);
	}

}



/**
 * Adds an Orbiter .msh format mesh to the physics world by reading it from a .msh file
 * Converts the triangle list in .msh -> btBvhTriangleMeshShape
 *
 */
btCollisionShape*
PhysicsEngine::importMsh(char *fileName)
{
	int i, g, numGroups = 0, materialIndex=0, textureIndex=0;
	int *nv, *nt;
	float x, y, z, nx, ny, nz;
	char buffer[50], label[50];
	FILE* fp;
	int **mshIndicesList;
	btVector3 **mshVerticesList;
	int totalVertices = 0, totalIndices = 0;

	//create a triangle-mesh
	int vertStride = sizeof(btVector3);
	int indexStride = 3*sizeof(int);
	const float TRIANGLE_SIZE=5.f;

	//printf("\n-------Reading msh file------\n");

	fopen_s(&fp, fileName,"r");
	if(fp == NULL){
		sprintf(boutput, "The MSH file \"%s\" could not be opened. Is it being held by another application ?",
				fileName); oapiWriteLog(boutput);
		return NULL;
	}

	fscanf_s(fp, "MSHX1\n");
	fscanf_s(fp, "GROUPS %d\n", &numGroups);
	printf("Number of groups = %d\n", numGroups);
	mshVerticesList = new btVector3*[numGroups];
	mshIndicesList  = new int*[numGroups];
	nv  = new int[numGroups];
	nt  = new int[numGroups];

	//Read in the vertices of all groups
	for(g=0; g<numGroups; g++){
		fscanf_s(fp, "LABEL ", buffer);
		fgets(label, 49, fp);
		fscanf_s(fp, "MATERIAL %d\n", &materialIndex);
		fscanf_s(fp, "TEXTURE %d\n", &textureIndex);
		fscanf_s(fp, "GEOM %d %d ;", &nv[g], &nt[g]);
		fgets(buffer, 49, fp);
		/*printf("------------Group = %d(Ignoring normals, materials, textures)-------------\n", g);
		printf("Label = %s", label);
		printf("Material Index = %d\n", materialIndex);
		printf("Texture Index = %d\n", textureIndex);
		printf("Number of vertices=%d, Number of Triangles=%d\n", nv[g], nt[g]);*/

		//Read in the vertices & normals for the current group in separate arrays
		mshVerticesList[g] = new btVector3[nv[g]];
		for(i=0; i<nv[g]; i++){
			fscanf_s(fp, "%f %f %f %f %f %f\n", &x,  &y,  &z, &nx, &ny, &nz);
			mshVerticesList[g][i].setValue(x, y, z);
		}

		//Read in the triangle indices for the current group in separate arrays BUT
		//offset the current group's indices by previously read total indices to have uniform triangle information
		mshIndicesList[g] = new int[nt[g]*3];
		for(i=0; i<nt[g]*3; i+=3)
			fscanf_s(fp, "%d %d %d\n", &mshIndicesList[g][i], &mshIndicesList[g][i+1], &mshIndicesList[g][i+2]);

	/*	for(i=0; i<nv[g]; i++)
			printf("[%d] %f %f %f \n", i,
			mshVerticesList[g][i].getX(),  mshVerticesList[g][i].getY(), mshVerticesList[g][i].getZ());

		for(i=0; i<nt[g]*3; i+=3)
			printf("[%d] %d %d %d\n", i, mshIndicesList[g][i], mshIndicesList[g][i+1], mshIndicesList[g][i+2]);*/

		totalVertices += nv[g];
		totalIndices += (nt[g]*3); //number of indices is no. of triangles * 3

		//printf("Total Vertices so far  : %d\n", totalVertices);
		//printf("Total Indices so far  : %d\n", totalIndices);
	}

	//Merge vertices and indices list into 1
	mshVertices = new btVector3[totalVertices];
	mshIndices  = new int[totalIndices];

	int vertexOffset = 0, indexOffset = 0;
	for(g=0; g<numGroups; g++){
		//Vertices copy
		for(i=0; i<nv[g]; i++){
			mshVertices[vertexOffset + i] = mshVerticesList[g][i];
		}

		//Indices copy
		for(i=0; i<nt[g]*3; i++){
			mshIndices[indexOffset + i] = mshIndicesList[g][i] + vertexOffset; //add offset of already added total vertices
		}

		vertexOffset += nv[g]; //important to do this last(see addition above)
		indexOffset += nt[g]*3;
	}

	//printf("-----Merge Complete of %d vertices and %d indices-----\n", totalVertices, totalIndices);
/*	for(i=0; i<totalVertices; i++)
			printf("[%d] %f %f %f \n", i, mshVertices[i].getX(),  mshVertices[i].getY(), mshVertices[i].getZ());

	for(i=0; i<totalIndices; i+=3)
		printf("[%d] %d %d %d\n", i, mshIndices[i], mshIndices[i+1], mshIndices[i+2]);

*/
	//Finally make the triangle mesh
	m_indexVertexArrays = new btTriangleIndexVertexArray(totalIndices/3,
			mshIndices,
			indexStride,
			totalVertices,(btScalar*) &mshVertices[0].x(),vertStride);

	bool useQuantizedAabbCompression = true;
	btCollisionShape* p = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression);

	fclose(fp);

	printf("\n-------Mesh read complete------\n\n");

	delete[] mshVerticesList;
	delete[] mshIndicesList;
	delete nv;
	delete nt;

	return p;
}


/**
 * Wrapper around importMsh() to easily test .msh based triangle meshes
 * Converts the btBvhTriangleMeshShape -> btRigidBody
 * This is for dynamic bodies(triangle meshes not recommended for dynamic objects)
 */
btRigidBody*
PhysicsEngine::addOrbiterMshDynamic(char *fileName, btVector3& offset)
{
	btCollisionShape* p = importMsh(fileName);
	m_collisionShapes.push_back(p);
	btTransform trans(btQuaternion(btVector3(0,1,0), (const btScalar)PI/2), offset);

	btDefaultMotionState* mshMotionState = new btDefaultMotionState(trans);
    btRigidBody::btRigidBodyConstructionInfo mshRigidBodyCI(0, mshMotionState, p, btVector3(0,0,0));
    btRigidBody* mshRigidBody = new btRigidBody(mshRigidBodyCI);
    m_dynamicsWorld->addRigidBody(mshRigidBody);

    return mshRigidBody;
}


/**
 * Wrapper around importMsh() to easily test .msh based triangle meshes
 * Converts the btBvhTriangleMeshShape -> btRigidBody
 * This is for static geometry
 */
btRigidBody*
PhysicsEngine::addOrbiterMshStatic(char *fileName, btVector3& offset)
{
	btCollisionShape* collShpMSH = importMsh(fileName);
	m_collisionShapes.push_back(collShpMSH);
	btTransform transMSH(btQuaternion(btVector3(0,1,0), (const btScalar)0), offset);

	return localCreateRigidBody(0.f, transMSH, collShpMSH);
}


/**
 * Defines a supporting plane to prevent vehicles from falling over the edge for
 * non-infinite ground shapes. This is more of an utility function and need not be made into a class
 */
void PhysicsEngine::addSupportPlane(void)
{
	supportPlaneShape = new btStaticPlaneShape(btVector3(0,1,0),0);
	m_collisionShapes.push_back(supportPlaneShape);
	btDefaultMotionState* supportMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),
																		btVector3(0,0,0)));
    btRigidBody::btRigidBodyConstructionInfo supportRigidBodyCI(0, supportMotionState,
    															   supportPlaneShape,
    															   btVector3(0,0,0));
    btRigidBody* supportRigidBody = new btRigidBody(supportRigidBodyCI);
    m_dynamicsWorld->addRigidBody(supportRigidBody);
}


/*
 * Creates dynamic bodies with a motion state
 * Required for kinematic objects used by VesselCollider.
 *
 */
btRigidBody*
PhysicsEngine::addRigidBody(float mass, const btTransform& startTransform,btCollisionShape* shape,
		btVector3& bodyInertia)
{
	btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE));

	//rigidbody is dynamic if and only if mass is non zero, otherwise static
	bool isDynamic = (mass != 0.f);

	//btVector3 localInertia(0,0,0);
	btRigidBody* body = NULL;
	if (isDynamic){
		//Using motionstate is recommended, it provides interpolation capabilities, and only synchronizes
		//'active' objects
		btDefaultMotionState* localMotionState = new btDefaultMotionState(startTransform);

		shape->calculateLocalInertia(mass, bodyInertia);

		btRigidBody::btRigidBodyConstructionInfo localRigidBodyCI(mass, localMotionState, shape, bodyInertia);
		body = new btRigidBody(localRigidBodyCI);
		m_dynamicsWorld->addRigidBody(body);
	}
	else{
		//No need for motion state for static bodies
		body = new btRigidBody(mass,0,shape,bodyInertia);
		body->setWorldTransform(startTransform);
		body->setContactProcessingThreshold(BT_LARGE_FLOAT);
		m_dynamicsWorld->addRigidBody(body);
	}

	return body;
}


/**
 * Creates dynamic bodies with a motion state
 * Required for kinematic objects used by VesselCollider.
 * The mass option is there for kinematic bodies even though they
 * should have 0 mass because the raycast vehicle needs non-zero mass
 * as its uses the action interface. This could be a bullet bug.
 *
 */
btRigidBody*
PhysicsEngine::addKinematicBody(btScalar mass, const btTransform& startTransform,btCollisionShape* shape)
{
	btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE));

	//mass should be 0 for kinematic objects, bullet calculates velocity of
	//kinematic objects using previous and current position(kept in motion state)
	btVector3 localInertia(0,0,0);

	//Using motionstate is necessary for kinematic objects
	btDefaultMotionState* localMotionState = new btDefaultMotionState(startTransform);

	//shape->calculateLocalInertia(mass, localInertia);

	btRigidBody::btRigidBodyConstructionInfo rbCI(mass, localMotionState, shape, localInertia);
	btRigidBody* body = new btRigidBody(rbCI);

	m_dynamicsWorld->addRigidBody(body);

	body->setCollisionFlags( body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);

	return body;
}


/**
 * GLUT callback  function to update physics, in the absence of GLUT will be called
 * directly from Bump to update the physics.
 */
void
PhysicsEngine::clientMoveAndDisplay(void)
{
	//updateVehicle();


	float dt;
	
	//dt = getDeltaTimeMicroseconds() * 0.000001f;
	dt = (float)oapiGetSimStep();

	//oapiWriteLogV("dt=%f", dt);

	if (m_dynamicsWorld)
	{
		//during idle mode, just run 1 simulation step maximum
		int maxSimSubSteps = m_idle ? 1 : 20;
		if (m_idle)
			dt = 1.0/420.f;

		int numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps);


	}

		

	//Gfx render
	//glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);



	//optional but useful: debug drawing
	/*if (m_dynamicsWorld)
		m_dynamicsWorld->debugDrawWorld();*/


	//glFlush();
	//swapBuffers();
	

}


/**
 * Used to reset the scene. This may not be a good idea when the engine is expanded
 * to dynamically add/remove objects
 * TODO: Figure out when reset makes sense, for the moment with a single car it does.
 */
void	PhysicsEngine::clientResetScene()
{
	initPhysics();
}


/**
 * Used in visualizer mode to render, by GLUT
 */
void PhysicsEngine::displayCallback(void)
{

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
	
	this->renderme();

	//optional but useful: debug drawing to detect problems
	if (m_dynamicsWorld)
		m_dynamicsWorld->debugDrawWorld();

	glFlush();
	swapBuffers();
}


/**
 * Added by Bump, to detect window close
 */
void PhysicsEngine::WMCloseFuncCallback(void)
{

}


/**
 * Called for special keys, when a key is released. Only used with GLUT
 */
void PhysicsEngine::specialKeyboardUp(int key, int x, int y)
{
   switch (key)
    {
/*    case GLUT_KEY_UP :
		{
			gEngineForce = 0.f;
		break;
		}
	case GLUT_KEY_DOWN :
		{
			gBreakingForce = 0.f;
		break;
		}*/
   case 1:	// to prevent the "no case defined" warning
   default:
		DemoApplication::specialKeyboardUp(key,x,y);
        break;
    }

}


/**
 * Called for special keys, when a key is released. Only used with GLUT
 */
void PhysicsEngine::specialKeyboard(int key, int x, int y)
{

//	printf("key = %i x=%i y=%i\n",key,x,y);

    switch (key)
    {

    // Keep these for debugging vehicle: there is a problem with vehicles every other day !
/*    case GLUT_KEY_LEFT :
		{
			gVehicleSteering += steeringIncrement;
			if (	gVehicleSteering > steeringClamp)
					gVehicleSteering = steeringClamp;

		break;
		}
    case GLUT_KEY_RIGHT :
		{
			gVehicleSteering -= steeringIncrement;
			if (	gVehicleSteering < -steeringClamp)
					gVehicleSteering = -steeringClamp;

		break;
		}
    case GLUT_KEY_UP :
		{
			m_carChassis->setActivationState(DISABLE_DEACTIVATION);
			gEngineForce = maxEngineForce;
			gBreakingForce = 0.f;
		break;
		}
	case GLUT_KEY_DOWN :
		{
			gBreakingForce = maxBreakingForce;
			gEngineForce = 0.f;
		break;
		}
*/

    case GLUT_KEY_F9 :
	{
		mCameraMode = VCM_FREE;
		break;
	}

    case GLUT_KEY_F10 :
		{
			mCameraMode = VCM_ORBITER;
			break;
		}

	case GLUT_KEY_F11 :
		{
			mCameraMode = VCM_GUI;
			break;
		}


  	default:
		DemoApplication::specialKeyboard(key,x,y);
        break;
    }

}


/**
 * Updates the camera position
 * TODO: Needs to be updated to work with multiple objects and
 * BulletBases.
 */
void PhysicsEngine::updateCamera()
{
	// Check which camera mode is active - only 2 for now
	if (mCameraMode == VCM_FREE) {
		DemoApplication::updateCamera();
		return;
	}

	btRigidBody *focusBody = mFocusBody[mCameraMode];
	btTransform transFocusBody;

	/*oapiWriteLogV("updateCamera : %p, focusBody : %p, mCameraMode:%d", mFocusBody[mCameraMode], focusBody,
			mCameraMode);*/


	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	
	if(focusBody){
		btMotionState *msFocusBody = focusBody->getMotionState();
		if(msFocusBody){
			msFocusBody->getWorldTransform(transFocusBody);
			m_cameraTargetPosition = transFocusBody.getOrigin();
		}
		else if (focusBody->getCollisionShape()->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) {
			btScalar rad;
			focusBody->getCollisionShape()->getBoundingSphere(m_cameraTargetPosition, rad);
		}
		else{
			transFocusBody = focusBody->getCenterOfMassTransform();
			m_cameraTargetPosition = transFocusBody.getOrigin();
		}


		//interpolate the camera height
		m_cameraPosition[1] = (15.0f *m_cameraPosition[1] + m_cameraTargetPosition[1] + m_cameraHeight)/16.0f;


		btVector3 camToObject = m_cameraTargetPosition - m_cameraPosition;

		//keep distance between min and max distance
		float cameraDistance = camToObject.length();
		float correctionFactor = 0.f;
		if (cameraDistance < m_minCameraDistance)
		{
			correctionFactor = 0.15f *(m_minCameraDistance-cameraDistance)/cameraDistance;
		}
		if (cameraDistance > m_maxCameraDistance)
		{
			correctionFactor = 0.15f *(m_maxCameraDistance-cameraDistance)/cameraDistance;
		}
		m_cameraPosition -= correctionFactor*camToObject;

		btScalar aspect = m_glutScreenWidth / (btScalar)m_glutScreenHeight;
		glFrustum (-aspect, aspect, -1.0, 1.0, 1.0, 10000.0);

		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();

		/*oapiWriteLogV("updateCamera : t:%f, %f, %f, cp:%f, %f, %f",
				BT3ARGS(m_cameraTargetPosition), BT3ARGS(m_cameraPosition));*/

		gluLookAt(m_cameraPosition[0],m_cameraPosition[1],m_cameraPosition[2],
				  m_cameraTargetPosition[0],m_cameraTargetPosition[1], m_cameraTargetPosition[2],
				  m_cameraUp.getX(),m_cameraUp.getY(),m_cameraUp.getZ());
	}

}


/**
 * Used to render wheels and stuff, useful for rendering stuff
 * not rendered by DemoApplication:renderme()
 * Also updates camera
 *
 * TODO: External mesh rendering which are not a part of the dynamics world like wheels
 */
void PhysicsEngine::renderme()
{

	updateCamera();


	btVector3 wheelColor(1,0,0), collisionColor(0.5,0.5,1);

	btVector3 worldBoundsMin, worldBoundsMax;
	getDynamicsWorld()->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);



	//External rendering functions for wheels should be inserted here
	for(renderObjIter = renderObjList.begin();  renderObjIter != renderObjList.end(); renderObjIter++){
		if((*renderObjIter))
			(*renderObjIter)->clbkPhysicsRender();
	}


	DemoApplication::renderme();

} 



/**
 * Prints the current state of the physics engine such as number, type, position of objects
 */
void PhysicsEngine::print(void)
{

	oapiWriteLog("----------- Bump Engine Details -----------");

	//Get these later
	//oapiWriteLogV("Time elapsed: %f", dt);
	//oapiWriteLog("Iterations completed: %ld", iterations);


	//List the rigid bodies in the dynamics world and their details
	int i;
	int numCollObjs = m_dynamicsWorld->getNumCollisionObjects();

	oapiWriteLogV("There are %d objects in the world", numCollObjs);

	for (i=numCollObjs-1; i>=0 ;i--)
	{
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];

		btCollisionShape *collShp = obj->getCollisionShape();
		int shpType = collShp->getShapeType();

		btRigidBody* body = btRigidBody::upcast(obj);
		btVector3 pos = body->getCenterOfMassPosition();

		oapiWriteLogV("%2d) Object is type:%d, pos:%f, %f, %f , ", i, shpType, BT3ARGS(pos));

	}

}

/**
 * Transform debugging function, can rip through the dynamics world and get transforms in the worst case
 * Used for investigating issues arising from incorrect extraction of transforms from the Bullet structs
 */
void
PhysicsEngine::getTransforms(void)
{
	btScalar m[16];
	const int numObjects = m_dynamicsWorld->getNumCollisionObjects();

	//Go through all collision objects
	for(int i=0;i<numObjects;i++){
		//Common properties
		btCollisionObject*	colObj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody*		body   = btRigidBody::upcast(colObj);
		const btCollisionShape* shape = colObj->getCollisionShape();

		if(body&&body->getMotionState()){
			btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();

			if(shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE){			//chassis
				myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(m);

				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
				btTransform childTrans = compoundShape->getChildTransform(0);
				//childTrans.getOpenGLMatrix(pkt.childMat);
				shape = compoundShape->getChildShape(0);
				if(shape->getShapeType() == BOX_SHAPE_PROXYTYPE){
					const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
					//pkt.halfExtent = boxShape->getHalfExtentsWithoutMargin(); //with and without margin makes a difference in size

				}
			}
			else if(shape->getShapeType() == BOX_SHAPE_PROXYTYPE){		//collision box
				//const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
				//myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(pkt.m_box);
				//pkt.box_halfExtent = boxShape->getHalfExtentsWithoutMargin();
			}

		}
		else{
			if(shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE){			//chassis
				colObj->getWorldTransform().getOpenGLMatrix(m);

				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
				btTransform childTrans = compoundShape->getChildTransform(0);
				//childTrans.getOpenGLMatrix(pkt.childMat);
				shape = compoundShape->getChildShape(0);
				if(shape->getShapeType() == BOX_SHAPE_PROXYTYPE){
					const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
					//pkt.halfExtent = boxShape->getHalfExtentsWithMargin();

				}
			}
			else if(shape->getShapeType() == BOX_SHAPE_PROXYTYPE){		//collision box
				//const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
				//colObj->getWorldTransform().getOpenGLMatrix(pkt.m_box);
				//pkt.box_halfExtent = boxShape->getHalfExtentsWithMargin();
			}
		}
	}
}


